
#include "BluetoothSerial.h"
#include <MPU6050_tockn.h>
#include <Wire.h>

BluetoothSerial SerialBT;
MPU6050 mpu6050(Wire);

int motor1=0;
int motor2=0;
int motor3=0;
int motor4=0;

float kp=5;
float ki=0.01;
float kd=0.01;

float preErr=0;
float iData=0;
float dData=0;

int specialShow=0;
int enabled=0;
int lauched=0;

float gas=0.0;

float doPID2(float set, float now, float drt){
  float dt = drt / 1000.0;
  float err = set - now;
  iData += err * dt;
  dData = (err - preErr) / dt;

  iData=constrain(iData, -50, 50);
  dData=constrain(dData, -50, 50);
  preErr = err;
  
  return kp * err + ki * iData + kd * dData;
}

float doPID(float set, float now,float acc){
  float err = set - now;
  iData += err;
  iData=constrain(iData, -50, 50);
  return kp * err + ki * iData + kd * acc;
}

void flyBalance(){
  int yaw=0;
  int pitch = doPID(0,mpu6050.getAngleX(),mpu6050.getGyroX());
  int roll = 0;

  int pwm = 80 * ( 1 + gas );
  motor1 = constrain(pwm + pitch + roll + yaw, 80, 255);
  motor2 = constrain(pwm + pitch - roll - yaw, 80, 255);
  motor3 = constrain(pwm - pitch + roll - yaw, 80, 255);
  motor4 = constrain(pwm - pitch - roll + yaw, 80, 255);

  Serial.println("mpu6050.getAngleX():"+String(mpu6050.getAngleX()));

  analogWrite(33, motor1);
  analogWrite(17, motor2);
  analogWrite(32, motor3);
  analogWrite(19, motor4);
}

void stop(){
  analogWrite(33, 0);
  analogWrite(17, 0);
  analogWrite(32, 0);
  analogWrite(19, 0);
}

void setup() {
  Serial.begin(9600);
  delay(1000);

  SerialBT.begin("ESP32_fly2");
  Wire.begin(22,23);
  mpu6050.begin();
  //mpu6050.calcGyroOffsets(true);

  analogWrite(25, 0);
  analogWriteFrequency(200000);
  //analogWriteResolution(uint8_t bits)

  stop();

  analogWrite(25, 0);
  analogWrite(18, 0);
  analogWrite(33, 200);
  analogWrite(17, 200);
  SerialBT.println("init done!");
  Serial.println("init done!");
}

void loop() {
  serial_debug();
  mpu6050.update();
  if(enabled==1){
    flyBalance();
  }
  delay(10);
}

void changeSpeed(char DATA){
  switch (DATA){
    case '0':
      stop();
      enabled = 0;
      break;
    case '1':
      enabled = 1;
      break;
    case '2':
      kp -= 1;
      break;
    case '3':
      kp += 1;
      break;
    case '4':
      ki -= 0.01;
      break;
    case '5':
      ki += 0.01;
      break;
    case '6':
      kd -= 0.01;
      break;
    case '7':
      kd += 0.01;
      break;
    case '8':
      gas += 0.1;
      break;
    case '9':
      gas -= 0.1;
      break;

    case 's':
      //
      break;
    case 'a':
      //
      break;
    case 'b':
      //
      break;
    case 'l':
      //
      break;
    case 'r':
      //
      break;
  }
  gas=constrain(gas,0,3.2);
  SerialBT.println("pwm:"+String(80 * ( 1 + gas ))+" p:"+String(kp)+" i:"+String(ki)+" d:"+String(kd));
}

String output_s="";

void serial_debug(){
  output_s="";
  while(SerialBT.available()){
    char a=SerialBT.read();
    output_s +=a;
    output_s.trim();
  }
  if(output_s!=""){
    SerialBT.println("output_s:"+output_s);
  }
}
